Methods and systems for mobile-agent navigation

ABSTRACT

Aspects of the present invention are related to methods and systems for autonomous navigation using visual-landmark recognition. One method may include scanning a captured image to detect a machine-readable marker and extracting encoded data therefrom. The data may include an identifying key usable to access a database to retrieve physical attributes associated with the marker. The method may include using the physical attributes to compute a position and an orientation of a mobile agent relative to a landmark object associated with the marker. The method may further include determining a path toward a next route location based on the position of the next route location and the computed position and orientation of the mobile agent and controlling the mobile agent to drive along the path toward the next route location.

CROSS REFERENCE TO RELATED APPLICATIONS

This application is a continuation-in-part of U.S. patent application Ser. No. 14/881,005, filed Oct. 12, 2015, which is a divisional of U.S. patent application Ser. No. 14/476,574, filed Sep. 3, 2014, which is now U.S. Pat. No. 9,157,757, by inventors Miao Liao, Xinyu Xu, and Petrus J. L. van Beek, the entire contents of which are hereby incorporated by reference, in their entirety for all purposes.

FIELD OF THE INVENTION

Embodiments of the present invention relate generally to methods and systems for autonomous navigation and, in particular, to methods and systems for autonomous navigation using visual-landmark recognition.

BACKGROUND

Several capabilities may be required for a mobile agent, for example, a vehicle, a robot and other mobile agents, to achieve autonomous navigation. For example, the mobile agent may require methods for determining its position, knowledge of its environment, methods for path planning and methods for obstacle avoidance. However, many autonomous navigation systems rely on high-end, expensive sensors, for example, laser scanners, high accuracy GPS receivers, orientation sensors and other high-end, expensive sensors, making these autonomous navigation systems unaffordable for many applications. Reliable, low-cost alternatives may be desirable.

SUMMARY

Some embodiments of the present invention comprise methods and systems for autonomous navigation using visual-landmark recognition.

According to a first aspect of the present invention, landmark-object detection may be used to identify a plurality of landmark objects. A distance and a bearing may be computed for each landmark object, and a nearest landmark object may be selected. The distance from the mobile agent to the nearest landmark object and the bearing angle of the mobile agent relative to the nearest landmark object may be used to compute a mobile-agent speed control value and a mobile-agent steering control value. The mobile-agent speed control value and the mobile-agent steering control value may be sent to a control component of the mobile agent effectuating movement of the mobile agent toward the nearest landmark object.

According to a second aspect of the present invention, a machine-readable marker may be associated with a landmark object and the machine-readable code may be used to compute a mobile-agent position and orientation relative to the landmark object. A route may be identified by a plurality of route-location identifiers, wherein each landmark-object identifier in the plurality of route-location identifiers may be associated with a position of a corresponding landmark object and autonomous navigation of the route may be effectuated.

According to a third aspect of the present invention, Visual Odometry may be used for monitoring and controlling the mobile agent to drive along a path.

The foregoing and other objectives, features, and advantages of the invention will be more readily understood upon consideration of the following detailed description of the invention taken in conjunction with the accompanying drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a chart showing exemplary embodiments of the present invention comprising visual-landmark detection, determination of a nearest landmark object and computation of a mobile-agent speed control value and a mobile-agent steering control value based on a distance to the nearest landmark object and a bearing of the nearest landmark object relative to the mobile agent;

FIG. 2 is chart showing exemplary embodiments of the present invention comprising visual-landmark detection;

FIG. 3A is a picture illustrating a distance from a mobile agent to a landmark object;

FIG. 3B is a picture illustrating a bearing, relative to a mobile agent, of a landmark object;

FIG. 4 is a picture illustrating a geometrical relationship between a camera onboard the mobile agent, an image plane, and a landmark object;

FIG. 5 is a picture of an exemplary binary landmark-object map generated in accordance with embodiments of the present invention;

FIG. 6 is a picture of exemplary landmark object and an exemplary associated machine-readable code in accordance with the present invention;

FIG. 7 is a picture of two exemplary landmark objects each comprising an integrated machine-readable code in accordance with the present invention;

FIG. 8 is picture illustrating exemplary physical attribute information stored in conjunction with a machine-readable code;

FIG. 9 is a chart showing exemplary embodiments of the present invention comprising a route that may be identified by a plurality of landmark-object identifiers, wherein each landmark-object identifier in the plurality of landmark-object identifiers may be associated with a position of a corresponding landmark object and autonomous navigation of the route defined by the ordered plurality of landmark-object identifiers;

FIG. 10 is a chart showing exemplary embodiments of the present invention comprising Visual Odometry for monitoring and controlling a turning angle;

FIG. 11 is a chart showing exemplary embodiments of the present invention comprising a route that may be identified by a plurality of landmark-object identifiers, wherein each landmark-object identifier in the plurality of landmark-object identifiers may be associated with a position of a corresponding landmark object and autonomous navigation of the route defined by the ordered plurality of landmark-object identifiers;

FIGS. 12A and 12B are a chart showing exemplary embodiments of the present invention comprising a route that may be identified by a plurality of route-location identifiers, wherein each landmark-object identifier in the plurality of route-location identifiers may be associated with a position of a corresponding landmark object and autonomous navigation of the route defined by the ordered plurality of route-location identifiers; and.

FIG. 13 is a picture illustrating an aerial view of an exemplary route including landmark objects and other route locations.

DETAILED DESCRIPTION OF EXEMPLARY EMBODIMENTS

Embodiments of the present invention will be best understood by reference to the drawings, wherein like parts are designated by like numerals throughout. The figures listed above are expressly incorporated as part of this detailed description.

It will be readily understood that the components of the present invention, as generally described and illustrated in the figures herein, could be arranged and designed in a wide variety of different configurations. Thus, the following more detailed description of the embodiments of the methods, systems and apparatus of the present invention is not intended to limit the scope of the invention, but it is merely representative of the presently preferred embodiments of the invention.

Elements of embodiments of the present invention may be embodied in hardware, firmware and/or a non-transitory computer program product comprising a computer-readable storage medium having instructions stored thereon/in which may be used to program a computing system. It will be appreciated that a computing system may include at least one processor configured to execute instructions stored in memory. While exemplary embodiments revealed herein may only describe one of these forms, it is to be understood that one skilled in the art would be able to effectuate these elements in any of these forms while resting within the scope of the present invention.

Although the charts and diagrams in the figures may show a specific order of execution, it is understood that the order of execution may differ from that which is depicted. For example, the order of execution of the blocks may be changed relative to the shown order. Also, as a further example, two or more blocks shown in succession in a figure may be executed concurrently, or with partial concurrence. It is understood by those with ordinary skill in the art that a non-transitory computer program product comprising a computer-readable storage medium having instructions stored thereon/in which may be used to program a computing system, hardware and/or firmware may be created by one of ordinary skill in the art to carry out the various logical functions described herein.

Several capabilities may be required for a mobile agent to achieve autonomous navigation. For example, the mobile agent may require methods for determining its position, knowledge of its environment, methods for path planning and methods for obstacle avoidance. However, many autonomous navigation systems rely on high-end, expensive sensors, for example, laser scanners, high accuracy GPS receivers, orientation sensors and other high-end, expensive sensors, making these autonomous navigation systems unaffordable for many applications. Reliable, low-cost alternatives may be desirable.

Some embodiments of the present invention comprise methods and systems for autonomous mobile-agent navigation based on one or a plurality of landmarks. A mobile agent may be equipped with a camera such as a forward-looking monocular camera, and a landmark, of the plurality of landmarks, may be a man-made visual guidepost, also referred to as a landmark object, with a unique color and/or shape pattern that may rarely be found in a natural scene. Landmark objects may be placed at known, geo-referenced positions during a site mapping/preparation stage. An ideal landmark object may be visually detected from a long range to provide a beacon for navigation. A detected landmark object may be visually tracked, thereby providing continuous guidance for mobile-agent autonomous navigation. Additionally, in some embodiments of the present invention, a landmark object may be associated with one or more identifiers that may link to geographic position data and other information about the landmark.

Some embodiments of the present invention comprising a method 100 for autonomous mobile-agent navigation may be described in relation to FIG. 1.

An image may be captured 102 using a camera such as a forward-looking monocular camera mounted on a mobile agent. In some embodiments of the present invention, the forward-looking monocular camera may acquire a color image. In alternative embodiments, the forward-looking monocular camera may acquire a gray-scale image, and in yet alternative embodiments, the forward-looking monocular camera may acquire both a color image and a gray-scale image. In other embodiments, the camera may be a stereo camera and the stereo camera may acquire two images from which depth information is extracted.

Landmark detection may be performed 104 to detect any landmark objects within the captured image. A determination may be made 106 as to whether or not one or more landmark objects are detected in the captured image. If no landmark object is detected 108, then the process may continue by capturing 102 a next image. If one or more landmark objects are detected 110, then for each detected landmark object, a distance to the landmark object and a bearing angle relative to the mobile agent may be computed 112. The nearest landmark object may be selected 114, and speed and steering control values, for the mobile agent, to effectuate driving toward the landmark may be computed 116. The control values may be sent 118 to a control component in the mobile agent, and the process may continue by capturing 102 a next image.

In some embodiments of the present invention, landmark detection 104 may be performed according to a method 200 described in relation to FIG. 2.

In these embodiments, a captured image may be received 202 in a landmark-detection processor component. Candidate landmark regions may be generated 204 from the received image. In some embodiments of the present invention, candidate-landmark-region generation may comprise color-based detection based on a color model, for example, an RGB color model, an HSV color model and other color models. Based on the color model, a likelihood of a pixel belonging to a landmark object may be evaluated on a pixel-by-pixel basis, thereby producing a pixel-likelihood map. Pixel-based thresholding may be performed on the pixel-likelihood map, thereby producing a binary object map. Connected-component analysis may be performed on the binary object map, thereby identifying candidate landmark regions. Small, proximately located candidate landmark regions may be merged 206 into a larger candidate landmark region, and filtering 208 of the resulting candidate landmark regions may be performed to remove any candidate landmark region that violates a pre-defined restriction on shape and/or size. In some embodiments of the present invention, a binary landmark-object map may be generated. One of the binary values may be associated with landmark-object pixels, and the other of the binary values may be associated with non-landmark-object pixels. The resulting candidate landmark regions may be associated 210 with existing landmarks.

In some embodiments of the present invention, a plurality of feature values may be computed for each candidate landmark region and compared to previously computed, stored feature values for each existing landmark. Exemplary features may include location, shape, size, color distribution, texture, edge content and other image features known in the art. When the feature values of a candidate landmark region match the feature values of an existing landmark, the candidate landmark region may be associated 210 with the existing landmark. A person having ordinary skill in the art will readily recognize many methods known in the art for performing feature matching.

The stored feature values of an existing landmark may be updated 212 when the existing landmark is associated with a candidate landmark region. The stored feature values of the existing landmark associated with a candidate landmark region may be updated 212 based on the feature values of the candidate landmark region. In some embodiments of the present invention, the stored feature values may be updated to the feature values of the candidate landmark region. In alternative embodiments, the stored feature values may be updated based on a weighting of the stored feature values and the feature values of the candidate landmark region.

Existing landmarks that are not associated with a candidate landmark may be marked 214 as unobserved. In some embodiments of the present invention, a counter may be associated with an existing landmark. In some embodiments of the present invention, the counter associated with the existing landmark may be initialized to zero and may be incremented for each captured image in which the landmark is unobserved. In alternative embodiments of the present invention, the counter associated with an existing landmark may be initialized to a non-zero value and may be decremented for each captured image in which the landmark is unobserved. When a landmark is unobserved for a predetermined number of captured images, the landmark may be considered dematerialized and may be deleted 216 from the record of existing landmarks.

If a candidate landmark region is not associated with an existing landmark, a new landmark, based on the unassociated candidate landmark region, may be created 218 and added to the record of existing landmarks. In some embodiments of the present invention, a newly added landmark may be differentiated in the record of existing landmarks from a landmark placed and generated during a site-mapping/route preparation stage.

Referring again to FIG. 1, if one or more, landmarks are detected 110, then for each detected landmark, a distance to the landmark and a bearing angle relative to the mobile agent may be computed 112.

FIG. 3A depicts an exemplary scene, shown from a side view 300, illustrating a distance computed in some embodiments of the present invention. A mobile agent 302 with an onboard camera 304 with projection center, C, 306 may capture an image of a landmark object 308. In some embodiments of the present invention, the camera 304 may be a forward-looking monocular camera or a stereo camera. In some embodiments of the present invention, the distance computed may be the distance, C′G, 310 between the projection, C′, 312 of the camera projection center, C, 306 on the ground and a point, G, 314 at which the landmark object 308 touches the ground. In alternative embodiments, the distance computed may be the distance, CG, 316 between the camera projection center, C, 306 and the point, G, 314 at which the landmark object 308 touches the ground. A top point 316 of the landmark object 308 may be denoted A.

FIG. 3B depicts the exemplary scene, shown in a side view in FIG. 3A, in a top view 350. FIG. 3B illustrates the relationship between the mobile agent 302, the camera 304, the camera projection center, C, 306, the landmark object 308 and a bearing angle 352.

FIG. 4 is a picture 400 that illustrates the geometrical relationship between the points shown in FIG. 3A and FIG. 3B to an image plane 402 with an x-dimension 404 and a y-dimension 406. The camera projection center, C, 408 and the projection, C′, 410 of the camera projection center, C, 408, on the ground are shown. The top point, A, 412 of the landmark object and the point, G, 414 at which the landmark object touches the ground are shown. The principle point, P, 416 of the image is shown in the image plane 402. The projection, A′, 418 of the top point, A, 412 of the landmark object is shown in the image plane 402. The projection, G′, 420 of the point, G, 414 at which the landmark object touches the ground is shown in the image plane 402. Additionally, a virtual point, D, 422 on the landmark object that has the same height as the camera, with respect to the ground, and its projection, E, 424 on the image plane 402 are shown.

In some embodiments of the present invention, the distance may be computed from the camera projection center, C, to the plane, which may be denoted Plane (AG), parallel to the image plane and containing the top point, A, of the landmark object and the point, G, at which the landmark object touches the ground. The distance, which may be denoted CPlane (AG), may be computed according to:

${{distance} = {{{CPlane}({AG})} = {{f \cdot \frac{H}{h}} = {f \cdot \frac{AG}{A^{\prime}G^{\prime}}}}}},$

where f may denote the camera focal length in pixels, H, may denote the landmark-object height from the ground in physical units, for example, meters, feet, yards and other physical units, and h may denote the landmark-object height on the image in pixels. The bearing angle may be computed according to:

${{{bearing}\mspace{14mu}{angle}} = {\theta = {{arc}\;{\tan\left( \frac{x - {xp}}{f} \right)}}}},$ where f may denote the camera focal length, x may denote the x-coordinate of the projection, A′, of the top point of the landmark object in the image plane and xp may denote the x-coordinate of the principle point, P, in the image plane. The camera focal length, f, and the x-coordinate, xp, of the principle point, P, may be determined by a camera calibration process, the landmark-object height, H, from the ground may be premeasured and the landmark-object height, h, on the image and the x-coordinate, x, of the projection, A′, of the top point of the landmark object in the image plane may be measured in the image as part of the distance computation. In these embodiments, a landmark object is assumed to be parallel to the camera image plane. When the camera image plane and the landmark object are perpendicular to the ground plane, this condition holds.

In alternative embodiments of the present invention, the distance may be determined by first computing the coordinates of G′ on the image plane. Denoting the coordinates of A′ on the image plane by [x, y], then the coordinates of G′ on the image plane may be given by [x, y+h], where h may denote the landmark-object height on the image. Then the length of PG′ may be computed according to: PG′=√{square root over ((xp−x)²+(yp−y−h)²)}, where [xp, yp] may denote the coordinates of P in the image plane. The length of CG′ may be computed using the right triangle CPG′ according to: CG′=√{square root over ((CP)²+(PG′)²)}=√{square root over (f ²+(PG′)²)}. Then the length of CG may then be computed using similar triangles CG′A′ and CGA according to:

${{CG} = {\frac{{CG}^{\prime} \cdot {AG}}{A^{\prime}G^{\prime}} = \frac{{CG}^{\prime} \cdot H}{h}}},$ and the length C′G may be computed using the right triangle CC′G according to: C′G=√{square root over ((CG)²−(CC′)²)}=√{square root over ((CG)² −H _(C))²)}, where H_(C) may denote the height of the camera above the ground and C′G is the distance of the mobile agent from the landmark object.

The bearing angle may be computed by first determining the camera height, EG′, on the image plane using similar triangle geometry according to:

$\frac{{EG}^{\prime}}{DG} = {\left. \frac{A^{\prime}G^{\prime}}{AG}\rightarrow{EG}^{\prime} \right. = {\frac{{{DG} \cdot A^{\prime}}G^{\prime}}{AG} = {\frac{H_{C} \cdot h}{H}.}}}$ The coordinate on the image plane of E may be computed according to [x,(y+h−EG′)] and used to compute the length of PE according to: PE=√{square root over ((xp−x)²+(yp−y−h+EG′)²)}. The angle between ray CP and CE may be computed using the right triangle CPE according to:

${\tan(\theta)} = {\left. \frac{PE}{CP}\rightarrow\theta \right. = \left\{ {\begin{matrix} {{{arc}\;{\tan\left( \frac{PE}{f} \right)}},} & {{if}\mspace{14mu} E\mspace{14mu}{is}\mspace{14mu}{to}\mspace{14mu}{the}\mspace{14mu}{right}\mspace{14mu}{of}\mspace{14mu} P} \\ {{{- {arc}}\;{\tan\left( \frac{PE}{f} \right)}},} & {{if}\mspace{14mu} E{\mspace{11mu}\;}{is}\mspace{14mu}{to}\mspace{14mu}{the}\mspace{14mu}{left}\mspace{14mu}{of}\mspace{14mu} P} \end{matrix}.} \right.}$

Referring again to FIG. 1, the nearest landmark object may be selected 114. In some embodiments of the present invention, the nearest landmark object may be the landmark object with the shortest distance between the mobile agent and the landmark object as computed according to embodiments of the present invention described above. In alternative embodiments, the nearest landmark object may be the landmark object with the largest area in the camera image. The landmark object with the largest area in the camera image may be determined by examining the binary landmark-object map. The number of pixels associated with each landmark object may be counted, and the landmark object associated with the greatest number of pixels may be selected as the nearest landmark object. FIG. 5 depicts an exemplary binary landmark-object map 500 comprising four landmark objects 502, 504, 506, 508. The largest landmark object 508 may be selected as the nearest landmark object according to embodiments of the present invention.

Mobile-agent speed and steering control values may be computed using the distance and bearing angle for the selected nearest landmark object. In some embodiments of the present invention, the mobile agent may require two driving control values for controlling the movement of the mobile agent: a forward speed and a steering parameter. The absolute value of the speed may define the moving velocity of the mobile agent. A negative value of the speed may make the mobile agent move in reverse. The absolute value of the steering parameter may define the rotational speed, also referred to as the turning speed, of the mobile agent. A positive steering parameter value may effectuate a turn to the right, and a negative steering parameter value may effectuate a turn to the left.

In some embodiments of the present invention, control of the mobile agent may comprise effectuating faster movement when the mobile agent is further from the target landmark object slower movement when the mobile agent is closer to the target landmark object. In some embodiments of the present invention, the forward speed of the mobile agent may be limited to a predetermined maximum value. In some embodiments of the present invention, a speed control value may be determined according to:

${speed} = \left\{ \begin{matrix} {{nomspeed},} & {{distance} > T_{2}} \\ {{\alpha*{nomspeed}},} & {{T_{2} > {distance} > T_{1}},} \\ {0,} & {{distance} \leq T_{1}} \end{matrix} \right.$ where α may denote a proportionality constant, nomspeed may denote a speed value associated with a normal driving speed of the mobile agent and T₁ and T₂ may be threshold values. In some exemplary embodiments of the present invention, T₁=3 meters, T₂=1 meter and α=0.5. In some embodiments of the present invention, the normal driving speed of the mobile agent may be related to one or more factors, for example, the maximum speed at which the mobile agent is capable of driving, the driving conditions, the terrain and other environmental conditions.

In some embodiments of the present invention, control of the mobile agent may be effectuated based on driving instructions. The driving instructions may be calculated based on motion measurements taken while the mobile agent is controlled to follow a determined path. The motion measurements may be taken by the camera 304, which may be monocular or stereo, or the mobile agent 302 may include an inertial measurement unit (IMU) comprising an accelerometer, gyroscope, and magnetometer. The motion measurements may provide information about whether the mobile agent 302 is accurately following the path or deviating therefrom while the mobile agent 302 is between landmark objects.

In some embodiments of the present invention, the rotational speed may be set to a higher value when the bearing angle relative to the target landmark object is large and to a lower value when the bearing angle relative to the target landmark object is small. In some embodiments of the present invention, a steering control value may be determined according to: steering=β*bearing, where β may denote a proportionality constant, and in some exemplary embodiments β=2.

Referring again to FIG. 1, the computed control values may be sent 118 to the control component of the mobile agent.

In some embodiments of the present invention, each landmark object may be associated with one or more unique machine-readable codes, for example, a bar-code, a QR-code, a Data Matrix code, an Aztec code and other machine-readable codes, in order to encode information specific to the landmark object.

In some embodiments of the present invention, a machine-readable marker may be embodied on a flat, planar surface. In alternative embodiments of the present invention, a machine-readable marker may be embodied on a non-planar surface. In some embodiments of the present invention, information (e.g., data) encoded in a machine-readable marker may be obtained by scanning the machine-readable marker, or an acquired image of the machine-readable marker, in a single, predefined direction. In alternative embodiments of the present invention, information encoded in a machine-readable marker may be obtained by scanning the machine-readable marker, or an acquired image of the machine-readable marker, in two, predefined directions. In some of these embodiments, the two, predefined directions may be orthogonal. In yet alternative embodiments of the present invention, information encoded in a machine-readable marker may be obtained by performing a de-warping, de-skewing or other perspective-distortion-correction process on an acquired image of the machine-readable marker, and scanning the corrected image.

In some embodiments of the present invention, a machine-readable marker may be a machine-readable code. QR-codes may be advantageous as the machine-readable markers due to several factors: a QR-code can encode a relatively large amount of information; the QR-code pattern may be scanned from a large viewing angle; QR-codes have error correction against noise; and a QR-code is easy to generate.

Some embodiments of the present invention may associate a QR-code with each landmark object. In some embodiments of the present invention, a QR code with a large barcode size and a high error-correction rate may be used. In some embodiments of the present invention, the QR-code associated with a landmark object may be replicated and presented on two perpendicular surfaces in proximity to the landmark object.

FIG. 6 depicts an exemplary QR-code/landmark object arrangement 600 conforming to exemplary embodiments of the present invention wherein the QR-code 602, 604 associated with a landmark object 610 may be replicated and presented on two perpendicular surfaces 606, 608 in proximity to the landmark object 610.

In some embodiments of the present invention, the unique machine-readable code may be integrated within the landmark object. For example, the landmark object may comprise a highly visible area of a specific color and shape, and the unique machine-readable code may be positioned within the highly visible area. FIG. 7 depicts two exemplary landmark objects 702, 704 with integrated QR-codes.

In some embodiments of the present invention, a unique machine-readable marker may itself be a landmark object. For these embodiments, the machine-readable marker/landmark object arrangement is the embodiment of the machine-readable marker in the physical environment, thus the landmark associated with the machine-readable marker is the machine-readable marker.

In some embodiments of the present invention, the unique machine-readable code may encode an identifying key that may be used to look up information associated with the unique machine-readable code and the associated landmark object. In some embodiments of the present invention, the information may be stored in a database. In some embodiments of the present invention, the database may be stored on the mobile agent. In alternative embodiments of the present invention, the database may be stored on a server. In yet alternative embodiments, the database may be stored on both the mobile agent and a server.

In alternative embodiments, information associated with the associated landmark object may be directly encoded within the associated machine-readable code.

Exemplary embodiments of the present invention may be described in relation to FIG. 8 and FIG. 9. In these exemplary embodiments, intended for illustration and are not to be considered limiting, a two-dimensional machine-readable code may be associated with a landmark object. An illustrative two-dimensional machine-readable code 800 is depicted in FIG. 8. The two-dimensional machine-readable code 800 may be square in dimension. In some embodiments of the present invention, an identifying key may be encoded on the two-dimensional machine-readable code 800. A length 802, which may be denoted L, of a side of the two-dimensional machine-readable code 800, a center point 804, which may be denoted (x,y,z), of the two-dimensional machine-readable code 800 and a normal vector 806, which may be denoted (a,b,c), to the plane of the two-dimensional machine-readable code 800 may be stored in a database at an entry associated with the identifying key for the two-dimensional machine-readable code 800. From the stored information, a vector 808 to a first edge 810 of the two-dimensional machine-readable code 800, a vector 812 to a second edge 814 of the two-dimensional machine-readable code 800 and four corner points 816, 818, 820, 822 of the two-dimensional machine-readable code 800 may be computed.

In some embodiments of the present invention, stored information and computed information may be in world coordinates and dimensions. In alternative embodiments of the present invention, stored information and computed information may be in a pre-defined coordinate system. In yet alternative embodiments of the present invention, the stored information and computed information may be denoted in a plurality of pre-defined coordinate systems.

In some embodiments of the present invention, an identifying key may be encoded on a machine-readable marker. A plurality of physical attributes of the machine-readable marker may be stored in a database at an entry associated with the identifying key. From the plurality of physical attributes, a plurality of locations, referred to as locating points, within the machine-readable marker may be computed. In some embodiments of the present invention, stored information and computed information may be in world coordinates and dimensions. In alternative embodiments of the present invention, stored information and computed information may be in a pre-defined coordinate system. In yet alternative embodiments of the present invention, the stored information and computed information may be denoted in a plurality of pre-defined coordinate systems.

In some embodiments of the present invention, a route may be identified by a plurality of landmark-object identifiers. The plurality of landmark-object identifiers may be stored in an order defining the route. Each landmark-object identifier in the plurality of landmark-object identifiers may be associated with a position of a corresponding landmark object. Some embodiments of the present invention may be described in relation to FIG. 9 and may comprise a method 900 for autonomous navigation of the route defined by the ordered plurality of landmark-object identifiers.

A captured image may be received 902. The received, captured image may be scanned 904 to locate a two-dimensional machine-readable code. A determination 906 may be made to determine whether or not a two-dimensional machine-readable code was detected in the received, captured image. If a two-dimensional machine-readable code was not detected 908, then the method may continue to receive a next captured image 902.

If a two-dimensional machine-readable code was detected 910, then the encoded data may be extracted 912. In some embodiments of the present invention, the extracted data may comprise an identifying key. The identifying key, or other extracted data, may be used to retrieve 914 information, for example, physical attributes and other information, associated with the two-dimensional machine-readable code. In some embodiments of the present invention, the retrieved physical attributes may comprise a length of a side of the two-dimensional machine-readable code, a center point of the two-dimensional machine-readable code and a normal vector to the plane of the two-dimensional machine-readable code.

From the retrieved, stored physical attributes, a vector to a first edge of the two-dimensional machine-readable code, a vector to a second edge of the two-dimensional machine-readable code and four corner points of the two-dimensional machine-readable code may be computed 916. The four corner points of the two-dimensional machine-readable code may be the locating points in this exemplary embodiment. All of the stored information and computed information may be in world coordinates and dimensions.

The locations, in the received, captured image, of the four corner points of the two-dimensional machine-readable code may be determined 918.

Using the four pairs of coordinates for the corner points, wherein each pair of coordinates comprises a world coordinate and an image coordinate, a rotation matrix and a translation matrix describing the pose of the camera relative to the landmark object associated with the two-dimensional machine-readable code may be computed 920. In some embodiments of the present invention, a Perspective-n-Point (PnP) algorithm, known in the art, may be used to compute the pose of the camera relative to the object.

The position of the next landmark object on the route may be obtained 922 from the plurality of landmark-object identifiers.

Using the position of the next landmark object on the route, the position of the current landmark object and the pose of the camera relative to the current landmark object, a turning angle for the mobile agent may be computed 924. The mobile agent may execute the turn and drive 926 toward the position of the next landmark object. The method may continue to receive a next captured image 902.

In some embodiments of the present invention, a four-capital-letter string, for example, AAAA, AAAB, BBBA and other four letter strings consisting of capital letters, may be used as an identifier key. The identifier key may be embedded within the machine-readable code. In some embodiments of the present invention, multiple machine-readable codes may be associated with one landmark object. In some of these embodiments, two identifiers may be used to identify a landmark object and the associated machine-readable codes. In alternative embodiments, a two-level identifier may be used. In yet alternative embodiments, a single identifier comprising two parts may be used. For example, a first portion of the identifier may identify the landmark object, and a second portion of the identifier may identify the machine-readable code.

In some embodiments of the present invention, the first two letters of a four-capital-letter string may identify the landmark object and the last two letters of the four-letter-capital-letter string may identify a machine-readable code associated with the landmark object. Records indexed by the identifying key may be stored in a database. The following records illustrate some exemplary records:

KEY AAAA:

-   -   ID: “AAAA”     -   L: 0.14     -   x: 0.     -   y: 0.     -   z: 0.     -   a: 1.     -   b: 0.     -   c: 0.

KEY AAAB:

-   -   ID: “AAAB”     -   L: 0.14     -   x: 0.     -   y: 0.     -   z: 0.     -   a: 0.     -   b: 0.     -   c: −1.

KEY BBBA:

-   -   ID: “BBBA”     -   L: 0.14     -   x: 0.     -   y: 0.     -   z: 10.     -   a: 0.     -   b: 0.     -   c: −1.

KEY BBBB:

-   -   ID: “BBBB”     -   L: 0.14     -   x: 0.     -   y: 0.     -   z: 10.     -   a: 1.     -   b: 0.     -   c: 0.

In these exemplary records, the information L, x, y, z, a, b and c correspond to a length, L, of a side of a two-dimensional machine-readable code, a center point, (x,y,z), of the two-dimensional machine-readable code and a normal vector, (a,b,c), to the plane of the two-dimensional machine-readable code, respectively. The identifiers “AAAA” and “AAAB” are associated with a first landmark object, and the identifiers “BBBA” and “BBBB” are associated with a second landmark object. In alternative embodiments of the present invention, additional information may be stored in each record.

In some embodiments of the present invention, Visual Odometry (VO) may be used to measure a turning angle of a mobile agent as the mobile agent executes a turn in preparation to drive to a next location within a route. In some embodiments of the present invention, described in relation to FIG. 10, when a mobile agent begins a turn, a VO measurement method may be initiated 1002. In some embodiments of the present invention, a VO measurement may be made in accordance with methods and systems described in U.S. patent application Ser. No. 14/476,524, entitled “Methods and Systems for Vision-Based Motion Estimation,” filed on Sep. 3, 2014 by inventors Xinyu Xu, Miao Liao and Petrus J. L. van Beek, the entire contents of which are hereby incorporated by reference, in their entirety for all purposes.

The measured angle 1004 indicating the rotation of the mobile agent may be accumulated between captured frames, and a determination may be made to determine 1006 if the accumulated angle is greater than the target angle, wherein the target angle may be the desired turning angle. If the accumulated angle is 1008 greater than the target angle, the mobile agent may stop 1010. If the accumulated angle is not 1012 greater than the target angle, the mobile agent may continue to turn, and the VO measurements may continue to accumulate measured angles.

In some embodiments of the present invention, a route may be identified by a plurality of landmark-object identifiers. The plurality of landmark-object identifiers may be stored in an order defining the route. Each landmark-object identifier in the plurality of landmark-object identifiers may be associated with a position of a corresponding landmark object. Some embodiments of the present invention may be described in relation to FIG. 11 and may comprise a method 1100 for autonomous navigation of the route defined by the ordered plurality of landmark-object identifiers.

A captured image may be received 1102. The received, captured image may be scanned 1104 to locate a machine-readable marker, for example, a two-dimensional machine-readable code, a bar code and other machine-readable codes. A determination 1106 may be made to determine whether or not a machine-readable marker was detected in the received, captured image. If a machine-readable marker was not detected 1108, then the method may continue to receive a next captured image 1102.

If a machine-readable marker was detected 1110, then the data encoded on the machine-readable marker may be extracted 1112. In some embodiments of the present invention, the extracted data may comprise an identifying key. The identifying key, or other extracted data, may be used to retrieve 1114 information, for example, physical attributes and other information, associated with the machine-readable marker. In some embodiments of the present invention, the retrieved information may comprise physical attributes of the machine-readable marker that may be used to compute locating points within the machine-readable marker. In some embodiments of the present invention, the locating points may comprise four corners of the machine-readable marker. In alternative embodiments of the present invention, the locating points may comprise three non-co-linear points within the machine-readable marker. In alternative embodiments, the locating points may comprise a plurality of points within the machine-readable marker that may be used to unambiguously determine the real-world location of the machine-readable marker.

From the retrieved information, a location for each locating point in a plurality of locating points may be computed 1116. All of the stored information and computed information may be in world coordinates and dimensions.

The locations, in the received, captured image, of the locating points in the plurality of locating points of the machine-readable code may be determined 1118.

Using the pairs of coordinates for the locating points, wherein each pair of coordinates comprises a world coordinate and an image coordinate, a rotation matrix and a translation matrix describing the pose of the camera relative to the landmark object associated with the machine-readable marker may be computed 1120. In some embodiments of the present invention, a Perspective-n-Point (PnP) algorithm, known in the art, may be used to compute the pose of the camera relative to the object.

The position of the next landmark object on the route may be obtained 1122 from the plurality of landmark-object identifiers.

Using the position of the next landmark object on the route, the position of the current landmark object, and the pose of the camera relative to the current landmark object, a path toward the next landmark object may be determined 1124. The mobile agent may be controlled to drive 1126 along the path toward the position of the next landmark object. The method may continue to receive a next captured image 1102.

In some embodiments of the present invention, a route may be identified by a plurality of route location-identifiers. The plurality of route location-identifiers may comprise a plurality of landmark-object identifiers and a plurality of non-landmark-object identifiers. The plurality of route location-identifiers may be stored in an order defining the route. Each landmark-object identifier in the plurality of landmark-object identifiers may be associated with a position of a corresponding landmark object. Each non-landmark-object identifier in the plurality of non-landmark-object identifiers may be associated with a non-landmark location on the route, where the non-landmark location may be a route location that is not a landmark location.

Some embodiments of the present invention may be described in relation to FIGS. 12A and 12B and may comprise a method 1200 for autonomous navigation of a route defined by an ordered plurality of route location-identifiers.

A captured image may be received 1202. The received, captured image may be scanned 1204 to locate a machine-readable marker. A determination 1206 may be made to determine whether or not a machine-readable marker was detected in the received, captured image. If a machine-readable marker was not detected 1208, then the method may continue to receive a next captured image 1202.

If a machine-readable marker was detected 1210, then the encoded data may be extracted 1212. In some embodiments of the present invention, the extracted data may comprise an identifying key. The identifying key, or other extracted data, may be used to retrieve 1214 information, for example, physical attributes and other information, associated with the machine-readable marker. In some embodiments of the present invention, the retrieved information may comprise physical attributes of the machine-readable marker that may be used to compute locating points within the machine-readable marker. In some embodiments of the present invention, the locating points may comprise the four corners of the machine-readable marker. In alternative embodiments of the present invention, the locating points may comprise three non-co-linear points within the machine-readable marker. In yet alternative embodiments of the present invention, the locating points may comprise a necessary and sufficient number and configuration of points within the machine-readable marker to effectuate the determination of mobile-agent position and orientation relative to an associated landmark object.

From the retrieved information, a location for each locating point in a plurality of locating points may be computed 1216. All of the stored information and computed information may be in world coordinates and dimensions.

The locations, in the received, captured image, of the locating points in the plurality of locating points of the machine-readable marker may be determined 1218.

Using the pairs of coordinates for the locating points, wherein each pair of coordinates comprises a world coordinate and an image coordinate, a rotation matrix and a translation matrix describing the pose of the camera relative to the landmark object associated with the machine-readable marker may be computed 1220. In some embodiments of the present invention, a Perspective-n-Point (PnP) algorithm, known in the art, may be used to compute the pose of the camera relative to the object.

Using the position 1222 of the next route-location identifier on the route, the position of the current landmark object, and the pose of the camera relative to the current landmark object, a path toward the next route location may be determined 1224. As an intermediate step, using the position of the current landmark object and the pose of the camera relative to the current landmark object, a current pose of the mobile agent may be determined. Then, the path may be determined based on the position of the next route-location identifier on the route and the current pose of the mobile agent. The mobile agent may be controlled to drive 1230 along the path toward the position of the next route location while monitoring 1236 to determine if the next route location has been reached. In some embodiments of the present invention, controlling 1230 the mobile agent may comprise taking 1232 repeated motion measurements during the controlling 1230 and determining 1234 driving instructions based on the motion measurements and the path. Motion measurements may be taken according the methods and systems described in U.S. patent application Ser. No. 14/476,524, which may include methods for motion measurement based on a monocular camera. However, other suitable methods may be used. For example, motion measurements may be obtained using a stereo camera comprising two cameras separated by a known offset and each having a known orientation relative to each other, the stereo camera configured to provide three dimensional or depth information. As another example, the mobile agent may include an inertial measurement unit (IMU) comprising an accelerometer, gyroscope, and magnetometer.

If the next route location has not 1238 been reached, then the mobile agent may be controlled to drive 1230 along the path toward the next route location. If the next route location has 1240 been reached, then a determination 1244 may be made to determine if the next route location is a landmark location. If the next route location is not 1246 a landmark location, then the mobile agent may navigate and drive 1230 along the path toward the next route location. If the next route location is 1248 a landmark location, then the method 1200 may continue by receiving a captured image 1202 and proceeding as described herein.

Some embodiments of the present invention may be described in relation to an exemplary view 1300 of FIG. 13. As discussed above, a route may include at least one landmark object. In the exemplary view 1300, a route 1302, shown as a dashed line, may include a first landmark object 1310, a second landmark object 1312, a third landmark object 1314, and a fourth landmark object 1316. The route 1302 may further include one or more route locations other than the landmark objects; as shown, the route 1300 may include a first route location 1320, a second route location 1322, a third route location 1324, and a fourth route location 1326. The mobile agent may determine a path 1304 from its current position (for example, after the first landmark object 1310) to a next route location (for example, the first route location 1320) according to any suitable above-described method.

As the mobile agent navigates the route 1302 according to any of the above-described methods, the mobile agent may periodically or constantly check the accuracy of its navigation by taking repeated motion measurements and determining driving instructions based on the motion measurements and the path 1304. The various route locations may be used as checkpoints to heighten navigation accuracy in between landmark objects. If the mobile agent is on track, then the path 1304 may simply be the portion of the route 1302 from its current location to the next route location. If the mobile agent has veered off track from the route 1302, then the path 1304 may begin deviated from the route 1302 and rejoin the route 1302 at the next route location. Thus, the path 1304 may be updated according to changing factors including navigation inaccuracy. As can be seen in the view 1300, when the mobile agent is controlled to drive along the route 1302, driving along the determined path may include a variety of movements including turning a corner, continuing straight forward, following a curved path, etc.

Some embodiments of the present invention may include a mobile agent comprising a camera for capturing an image, a control component, and a processor. The processor may be configured to scan said captured image to detect a machine-readable marker, when a machine-readable marker is detected, extract an encoded data from said detected machine-readable marker, wherein said extracted encoded data comprises an identifying key, using said identifying key, access a database to retrieve a plurality of physical attributes associated with said detected machine-readable marker, using said plurality of physical attributes, compute a world coordinate for each locating point in a plurality of locating points associated with said detected machine-readable marker, determine a corresponding image coordinate for said each locating point in said plurality of locating points, and using said world coordinate for said each locating point in said plurality of locating points and said corresponding image coordinate for said each locating point in said plurality of locating points, compute a position and an orientation of the mobile agent relative to a landmark object associated with said detected machine-readable marker.

Some embodiments of the present invention may include said processor being further configured to obtain a position of a next route location and determine a path toward said next route location based on said position of said next route location and said position and said orientation of said mobile agent relative to said landmark object associated with said detected machine-readable marker. Said control portion may be configured to control said mobile agent to drive along said path toward said next route location. Said processor may be configured to obtain a position of a next route location by accessing a stored route comprising a plurality of landmark-object identifiers. Said processor may be configured to obtain a position of a next route location by accessing a stored route comprising a plurality of route-location identifiers. Said processor may be further configured to take repeated motion measurements during said controlling, and determine driving instructions based on said motion measurements and said path.

Some embodiments of the present invention may include said database being on said mobile agent, or said database being on a server. Said machine-readable marker may be a code selected from the group consisting of a barcode, a QR code, an Aztec code, and a Data Matrix code. Said plurality of physical attributes may comprise three non-co-linear points on said machine-readable marker.

Some embodiments of the present invention may comprise a computer program product comprising a computer-readable storage medium having instructions stored thereon/in which may be used to program a computing system to perform any of the features and methods described herein. Exemplary computer-readable storage media may include, but are not limited to, flash memory devices, disk storage media, for example, floppy disks, optical disks, magneto-optical disks, Digital Versatile Discs (DVDs), Compact Discs (CDs), micro-drives and other disk storage media, Read-Only Memory (ROMs), Programmable Read-Only Memory (PROMs), Erasable Programmable Read-Only Memory (EPROMS), Electrically Erasable Programmable Read-Only Memory (EEPROMs), Random-Access Memory (RAMS), Video Random-Access Memory (VRAMs), Dynamic Random-Access Memory (DRAMs) and any type of media or device suitable for storing instructions and/or data. The terms and expressions which have been employed in the foregoing specification are used therein as terms of description and not of limitation, and there is no intention in the use of such terms and expressions of excluding equivalence of the features shown and described or portions thereof, it being recognized that the scope of the invention is defined and limited only by the claims which follow. 

The invention claimed is:
 1. A method for autonomous navigation, said method comprising: receiving a captured image; scanning said captured image to detect a machine-readable marker; when a machine-readable marker is detected: extracting an encoded data from said detected machine-readable marker, wherein said extracted encoded data comprises an identifying key; using said identifying key, accessing a database to retrieve a plurality of physical attributes associated with said detected machine-readable marker; using said plurality of physical attributes, computing a world coordinate for each locating point in a plurality of locating points associated with said detected, machine-readable marker; determining a corresponding image coordinate for said each locating point in said plurality of locating points; and using said world coordinate for said each locating point in said plurality of locating points and said corresponding image coordinate for said each locating point in said plurality of locating points, computing a position and an orientation of a mobile agent relative to a landmark object associated with said detected machine-readable marker.
 2. The method of claim 1, further comprising: obtaining a position of a next route location; determining a path toward said next route location based on said position of said next route location and said position and said orientation of said mobile agent relative to said landmark object associated with said detected machine-readable marker; controlling said mobile agent to drive along said path toward said next route location.
 3. The method of claim 2, wherein said obtaining a position of a next route location comprises accessing a stored route comprising a plurality of landmark-object identifiers.
 4. The method of claim 2, wherein said obtaining a position of a next route location comprises accessing a stored route comprising a plurality of route-location identifiers.
 5. The method of claim 2, further comprising: taking repeated motion measurements during said controlling; and determining driving instructions based on said motion measurements and said path.
 6. The method of claim 1, wherein said computing a position and an orientation of a mobile agent relative to a landmark object associated with said detected machine-readable marker comprises a Perspective-n-Point method.
 7. The method of claim 1, wherein said machine-readable marker is a code selected from the group consisting of a barcode, a QR code, an Aztec code, and a Data Matrix code.
 8. The method of claim 1, wherein: said machine-readable marker is a two-dimensional machine-readable code; and said plurality of physical attributes comprises a length of a side of said two-dimensional machine-readable code, a world coordinate associated with a center of said two-dimensional machine-readable code, and a normal vector associated with said two-dimensional machine-readable code.
 9. The method of claim 1, wherein said database is on said mobile agent.
 10. The method of claim 1, wherein said database is on a server.
 11. The method of claim 1, wherein said plurality of physical attributes comprises three non-co-linear points on said machine-readable code.
 12. A mobile agent comprising: a camera for capturing an image; a control component; and a processor configured to: scan said captured image to detect a machine-readable marker; when a machine-readable marker is detected: extract an encoded data from said detected machine-readable marker, wherein said extracted encoded data comprises an identifying key; using said identifying key, access a database to retrieve a plurality of physical attributes associated with said detected machine-readable marker; using said plurality of physical attributes, compute a world coordinate for each locating point in a plurality of locating points associated with said detected machine-readable marker; determine a corresponding image coordinate for said each locating point in said plurality of locating points; and using said world coordinate for said each locating point in said plurality of locating points and said corresponding image coordinate for said each locating point in said plurality of locating points, compute a position and an orientation of the mobile agent relative to a landmark object associated with said detected machine-readable marker.
 13. The mobile agent of claim 12, wherein said processor is further configured to: obtain a position of a next route location; and determine a path toward said next route location based on said position of said next route location and said position and said orientation of said mobile agent relative to said landmark object associated with said detected machine-readable marker; and said control portion is configured to control said mobile agent to drive along said path toward said next route location.
 14. The mobile agent of claim 13, wherein said processor is configured to obtain a position of a next route location by accessing a stored route comprising a plurality of landmark-object identifiers.
 15. The mobile agent of claim 13, wherein said processor is configured to obtain a position of a next route location by accessing a stored route comprising a plurality of route-location identifiers.
 16. The mobile agent of claim 13, wherein said processor is further configured to: take repeated motion measurements during said controlling; and determine driving instructions based on said motion measurements and said path.
 17. The mobile agent of claim 12, wherein said database is on said mobile agent.
 18. The mobile agent of claim 12, wherein said database is on a server.
 19. The mobile agent of claim 12, wherein said machine-readable marker is a code selected from the group consisting of a barcode, a QR code, an Aztec code, and a Data Matrix code.
 20. The mobile agent of claim 12, wherein said plurality of physical attributes comprises three non-co-linear points on said machine-readable marker. 